#include "Calibration.h"

using namespace std;
using namespace cv;
using namespace Eigen;

//////////全局变量，因为需要将两次标定的数据保存下来使用/////////
vector<Mat> cameraMatrix(2, Mat(3, 3, CV_32FC1, Scalar::all(0)));
vector<Mat> distCoeffs(2, Mat(3, 3, CV_32FC1, Scalar::all(0)));//摄像机的5个畸变系数k1,k2,p1,p2,k3
vector<vector<Mat> > tvecsMat(2);//每幅图像的 改正-》平移 向量
vector<vector<Mat> > rvecsMat(2);//每幅图像的 改正-》旋转 向量
vector<vector<MatrixXd> > rotation_matrix(2);
////////////////////////////////////////////////////////////////

calibration::calibration()
{
}

calibration::~calibration()
{
}

void calibration::CalibrationPic(std::vector<cv::Mat>& pictures ,int picflag)
{
	Size ImageSize;//图片尺寸
	Size BoardSize = Size(6,9);//标定板上每行、每列的角点数
	vector<Point2f> image_points_buf;//缓存每幅图像上检测到的角点	
        vector<int> savefail;
	int AngleCount = -1;//用于存储角点数
	int PictureNum = pictures.size();
	int PicIndex = 0;
	Mat Romatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//保存每幅图像的旋转矩阵

	object_points.clear();
	image_points_seq.clear();
        savefail.clear();

	
	cout << "开始提取角点....." << endl;

	ImageSize.width = pictures[0].cols;
	ImageSize.height = pictures[0].rows;
	cout << "ImageSize.width = " << ImageSize.width << endl;
	cout << "ImageSize.height = " << ImageSize.height << endl;


	for(int i = 0; i < pictures.size(); i++)
	{
		
		
     	//提取角点
		if (0 == findChessboardCorners(pictures[i], BoardSize, image_points_buf))
		{
			cout << "can not find chessboard  "<< i << "  corners!\n";
                        savefail.push_back(i);
			//exit(1);
		}
		else
		{
			Mat view_gray;
			cvtColor(pictures[i], view_gray, COLOR_BGR2GRAY);
			find4QuadCornerSubpix(view_gray, image_points_buf, Size(10, 10));//亚像素精确化
			image_points_seq.push_back(image_points_buf);//保存亚像素点

			//图像上显示角点位置

			drawChessboardCorners(view_gray, BoardSize, image_points_buf, false);

			namedWindow("Canmera Calibration");
			imshow("Canmera Calibration", view_gray);
			waitKey(500);
		    destroyWindow("Canmera Calibration");
		}
		
	}


	cout << "total = " << image_points_seq.size() << endl;

	cout << endl << "角点提取完成！" << endl;

	//以下是摄像机标定
	cout << "开始标定....." << endl;
	//棋盘三维信息
	Size square_size = Size(26, 26);//实际测量得到的标定板上每个棋盘格的大小
	//vector<vector<Point3f>> object_points;

	vector<int> point_counts;//每幅图像中角点的数量

	//初始化标定板上角点的三维坐标
	int i, j, t;
	for (t = 0; t < pictures.size(); t++)
	{
		vector<Point3f> tempPointSet;
                if(find(savefail.begin(), savefail.end(),t) == savefail.end())
                {
		        for (i = 0; i < BoardSize.height; i++)
			{
				for (j = 0; j < BoardSize.width; j++)
				{
					Point3f realPoint;
					//假设标定板放在世界坐标系中Z=0的平面上
					realPoint.x = i * square_size.width;
					realPoint.y = j * square_size.height;
					realPoint.z = 0;
					tempPointSet.push_back(realPoint);
				}
			}
			object_points.push_back(tempPointSet);

                }
                else
                {}

	}
	//初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板
	for (i = 0; i < pictures.size(); i++)
	{
		point_counts.push_back(BoardSize.width*BoardSize.height);
	}
	//开始标定
	calibrateCamera(object_points, image_points_seq, ImageSize, cameraMatrix[picflag], distCoeffs[picflag], rvecsMat[picflag], tvecsMat[picflag], 0);
	cout << "标定完成~！" << endl;

	//对标定结果进行评价
	cout << "开始评价标定结果......" << endl;
	double total_err = 0.0;//所有图像的平均误差的总和
	double err = 0.0;//每幅图像的平均误差
	vector<Point2f> image_points2;//保存重新计算得到的投影点
	cout << "\t每幅图像的标定误差:" << endl;
	for (i = 0; i < pictures.size(); i++)
	{
                if(find(savefail.begin(), savefail.end(),t) == savefail.end())
                {
					vector<Point3f> tempPointSet = object_points[i];
					//通过得到的摄像机内外参数，对空间的三维点进行重新投影计算,rvecsMat平移向量
					projectPoints(tempPointSet, rvecsMat[picflag][i], tvecsMat[picflag][i], cameraMatrix[picflag], distCoeffs[picflag], image_points2);
					//计算心得投影点和旧的投影点之间的误差
					vector<Point2f> tempImagePoint = image_points_seq[i];
					Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
					Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);
					for (int j = 0; j < tempImagePoint.size(); j++)
					{
						image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
						tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
					}
					err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
					total_err += err /= point_counts[i];
					cout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
                }
                else
                {}

	}
	cout << "总体平均误差：" << total_err / (pictures.size() - savefail.size()) << "像素" << endl;
    cout << "评价完成!" << endl;

}

cv::Mat calibration::CorrectPic(cv::Mat frame)
{
	cout << "矫正图片...." << endl;
	Size BoardSize = Size(6, 9);//标定板上每行、每列的角点数
	Size image_size;
	Mat correctFrame = frame.clone();
	Mat correct = frame.clone();
	image_size.width = correctFrame.cols;
	image_size.height = correctFrame.rows;
	Mat mapx = Mat(image_size, CV_32FC1);
	Mat mapy = Mat(image_size, CV_32FC1);
	Mat R = Mat();//Mat::eye(3, 3, CV_32F);
  

	distCoeffs[0].at<double>(0, 4) = 0.0;
	initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R, getOptimalNewCameraMatrix(cameraMatrix[0], distCoeffs[0], image_size, 1, image_size, 0), image_size, CV_32FC1, mapx, mapy);
	remap(correctFrame, correct, mapx, mapy, INTER_LINEAR);

	namedWindow("correct");
	imshow("correct", correct);
	waitKey(500);

	imageFileName.clear();
	StrStm.clear();
	StrStm << ++croNum;
	StrStm >> imageFileName;
	imageFileName += "_d.jpg";
	imwrite(imageFileName, correct);

	return correct;
}




cv::Point2d calibration::TableMap(float X, float Y ,MatrixXd HMatrix)
{	
	Point2d result;


	a1 = HMatrix(0,0) - HMatrix(2, 0) * X;
	a2 = HMatrix(0, 1) - HMatrix(2, 1) * X;
	a3 = HMatrix(0, 2) - HMatrix(2, 2) * X- HMatrix(2, 3) * X + HMatrix(0, 3);
	a4 = HMatrix(1, 0) - HMatrix(2, 0) * Y;
	a5 = HMatrix(1, 1) - HMatrix(2, 1) * Y;
	a6 = HMatrix(1, 2) - HMatrix(2, 2) * Y - HMatrix(2, 3) * Y + HMatrix(1, 3);

	result.y = (a3 * a4 - a1 * a6) / (a1 * a5 - a2 * a4);
	result.x = -1 * (a2 * result.y + a3) / a1;

	return result;
}

Eigen::MatrixXd calibration::CalculateHmatrix(int imageNum, int picflag)
{
	MatrixXd InMatrix(3, 3);
	MatrixXd OutMatrix(3, 4);
	MatrixXd HMatrix(3, 4);

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			InMatrix(i, j) = cameraMatrix[picflag].at<double>(i, j);
		}
	}
	for (int i = 0; i < PICTURENUM; i++)
	{
		Rodrigues(rvecsMat[picflag][i], Romatrix);

		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				OutMatrix(i, j) = Romatrix.at<double>(i, j);

			}
		}
		OutMatrix(0, 3) = tvecsMat[picflag][i].at<double>(0);
		OutMatrix(1, 3) = tvecsMat[picflag][i].at<double>(1);
		OutMatrix(2, 3) = tvecsMat[picflag][i].at<double>(2);

		rotation_matrix[picflag].push_back(OutMatrix);
	}

	HMatrix = InMatrix * rotation_matrix[picflag][imageNum];

	return HMatrix;
}

void calibration::testResult(int picflag)
{
	bool judge;
	MatrixXd HMatrix;
	for (int j = 0; j < object_points.size(); j++)
	{
		HMatrix = CalculateHmatrix(j, picflag);
		cout << "第" << j + 1 << "幅图像的误差：" << endl;
		for (int i = 0; i < 54; i++)
		{
			Point2d result;
			result = TableMap(image_points_seq[j][i].x, image_points_seq[j][i].y, HMatrix);
			cout << "X err " << result.x - object_points[j][i].x;
			cout << "  Y err " << result.y - object_points[j][i].y << endl;
			judge = false;
		}
	}
	
}

void calibration::helper(std::vector<cv::Mat>& pictures)
{

	pictures.clear();

	for (int i = 0; i < 15; i++)
	{
		Mat frame;
		StrStm << i + 1;
		StrStm >> imageFileName;
		filePath += imageFileName;
		filePath += "_d.jpg";
		frame = imread(filePath);
		pictures.push_back(frame.clone());
		StrStm.clear();
		imageFileName.clear();
		filePath.clear();

	}
}


void calibration::GetCorpictures(vector<Mat>& pictures, vector<Mat> & Corpictures)
{
	for (int i = 0; i < PICTURENUM; i++)
	{
		Corpictures.push_back(CorrectPic(pictures[i]).clone());
	}
	destroyWindow("correct");
}
